package com.wltt.product.frfront.executors;

import com.wltt.product.frfront.constants.WeldTypeConstant;
import com.wltt.product.frfront.enums.PositionTypeEnum;
import com.wltt.product.frfront.enums.WeldButtonIdEnum;
import com.wltt.product.frfront.holder.JavafxHolder;
import com.wltt.product.frfront.holder.RobotHolder;
import com.wltt.product.frfront.model.bo.FormulaDetailBO;
import com.wltt.product.frfront.model.bo.WeldPointBO;
import com.wltt.product.frfront.model.bo.WeldingJobBO;
import com.wltt.product.frfront.model.po.SwingTypePO;
import com.wltt.product.frfront.model.vo.CommonWeldREQ;
import fairino.DescPose;
import fairino.ExaxisPos;
import fairino.Robot;
import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;

import java.math.BigDecimal;
import java.math.RoundingMode;
import java.util.List;

public class CommonGroupWeldExecutor implements Runnable {

    private static final Logger logger = LogManager.getLogger(CommonGroupWeldExecutor.class);

    /**
     * 机器人对象
     */
    private Robot robot;

    /**
     * 运动参数
     */
    private CommonWeldREQ weldREQ;
    private FormulaDetailBO formulaDetailBO;
    private SwingTypePO swingTypePO;
    private List<WeldingJobBO> weldingJobList;

    public CommonGroupWeldExecutor(Robot robot, CommonWeldREQ weldREQ, FormulaDetailBO formulaDetailBO, SwingTypePO swingTypePO, List<WeldingJobBO> weldingJobList) {
        this.robot = robot;
        this.weldREQ = weldREQ;
        this.formulaDetailBO = formulaDetailBO;
        this.swingTypePO = swingTypePO;
        this.weldingJobList = weldingJobList;
    }

    @Override
    public void run() {
        if (!RobotHolder.setBusy()) {
            return;
        }
        if (RobotHolder.isStopWeld()) {
            return;
        }
        boolean weave = false;
        boolean ARC = false;
        try {
            JavafxHolder.setStartWeldDisable(WeldButtonIdEnum.COMMON_GROUP_WELD.getCode(), true);
            robot.Mode(0);
            SpeedBO speedBO = WeldTool.calculateSpeed(weldREQ.getWeldingSpeed());
            robot.SetSpeed(speedBO.getGlobalSpeedPercent());
            int vel = speedBO.getMoveSpeedPercent();

            WeldPointBO startSecurePoint;
            WeldPointBO endSecurePoint;
            WeldPointBO arcStartPoint;
            WeldPointBO arcEndPoint;
            List<WeldPointBO> jointPosList;
            for (WeldingJobBO weldingJobBO : weldingJobList) {
                if (RobotHolder.isStopWeld()) {
                    WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                    return;
                }

                startSecurePoint = weldingJobBO.getStartSecurePoint();
                endSecurePoint = weldingJobBO.getEndSecurePoint();
                arcStartPoint = weldingJobBO.getArcStartPoint();
                arcEndPoint = weldingJobBO.getArcEndPoint();
                jointPosList = weldingJobBO.getJointPosList();

                // 运动到开始安全点
                if (null != startSecurePoint) {
                    // 运动到开始安全点
                    DescPose currentDescPose =  new DescPose();
                    robot.GetActualTCPPose(currentDescPose);
                    if (new BigDecimal(currentDescPose.tran.x).setScale(3, RoundingMode.HALF_UP).doubleValue() != startSecurePoint.getDescPose().tran.x
                            || new BigDecimal(currentDescPose.tran.y).setScale(3, RoundingMode.HALF_UP).doubleValue() != startSecurePoint.getDescPose().tran.y
                            || new BigDecimal(currentDescPose.tran.z).setScale(3, RoundingMode.HALF_UP).doubleValue() != startSecurePoint.getDescPose().tran.z
                            || new BigDecimal(currentDescPose.rpy.rx).setScale(3, RoundingMode.HALF_UP).doubleValue() != startSecurePoint.getDescPose().rpy.rx
                            || new BigDecimal(currentDescPose.rpy.ry).setScale(3, RoundingMode.HALF_UP).doubleValue() != startSecurePoint.getDescPose().rpy.ry
                            || new BigDecimal(currentDescPose.rpy.rz).setScale(3, RoundingMode.HALF_UP).doubleValue() != startSecurePoint.getDescPose().rpy.rz) {
                        if (RobotHolder.isStopWeld()) {
                            WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                            return;
                        }
                        robot.MoveL(startSecurePoint.getJointPos(), startSecurePoint.getDescPose(),RobotHolder.getRobot().getRobotPO().getToolCoordinateNum(), RobotHolder.getRobot().getRobotPO().getWorkpieceCoordinateNum(), 100, 100, 100, -1, new ExaxisPos(), 0, 0, new DescPose(), 0, 100);
                    }
                }

                // 运动到起弧点
//                robot.MoveJ(arcStartPoint.getJointPos(), arcStartPoint.getDescPose(),1, 0, 100, 100, 100, new ExaxisPos(), 0, 0, new DescPose());
                if (RobotHolder.isStopWeld()) {
                    WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                    return;
                }
                robot.MoveL(arcStartPoint.getJointPos(), arcStartPoint.getDescPose(),RobotHolder.getRobot().getRobotPO().getToolCoordinateNum(), RobotHolder.getRobot().getRobotPO().getWorkpieceCoordinateNum(), 100, 100, 100, -1, new ExaxisPos(), 0, 0, new DescPose(), 0, 100);

                // 开始摆动
                if (weldREQ.getSwingEnabled()) {
                    robot.WeaveOnlineSetPara(RobotHolder.getRobot().getRobotPO().getWeaveConfigNum(),Integer.parseInt(swingTypePO.getSwingTypeCode()), formulaDetailBO.getSwingFrequency(),
                            Integer.parseInt(formulaDetailBO.getWaitModeCode()), formulaDetailBO.getSwingRange(), formulaDetailBO.getSwingLeftRemainTime(), formulaDetailBO.getSwingRightRemainTime(), formulaDetailBO.getCircleSwingPullbackRatio(), Integer.parseInt(formulaDetailBO.getSwingLocationWaitCode()));
                    if (RobotHolder.isStopWeld()) {
                        WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                        return;
                    }
                    robot.WeaveStart(RobotHolder.getRobot().getRobotPO().getWeaveConfigNum());
                    weave = true;
                }

                // 开始焊接
                if (weldREQ.getWeldEnabled()) {
                    robot.WeldingSetCurrent(0, weldREQ.getWeldingCurrent(), 0, 0);
                    robot.WeldingSetVoltage(0, weldREQ.getWeldingVoltage(), 1, 0);
                    if (RobotHolder.isStopWeld()) {
                        WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                        return;
                    }
                    robot.ARCStart(0, RobotHolder.getRobot().getRobotPO().getArcConfigNum(), 10000);
                    ARC = true;
                }

                if (WeldTypeConstant.Line == weldingJobBO.getWeldType()) {
                    // 焊接路径
                    for (int i = 0; i < jointPosList.size(); i++) {
                        WeldPointBO weldPointBO = jointPosList.get(i);
                        if (PositionTypeEnum.PASS.getCode() == weldPointBO.getPositionType()) {
                            if (RobotHolder.isStopWeld()) {
                                WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                                return;
                            }
                            robot.MoveL(weldPointBO.getJointPos(), weldPointBO.getDescPose(),RobotHolder.getRobot().getRobotPO().getToolCoordinateNum(), RobotHolder.getRobot().getRobotPO().getWorkpieceCoordinateNum(), vel, 100, 100, -1, new ExaxisPos(), 0, 0, new DescPose(), 0, 100);
                            if (RobotHolder.isStopWeld()) {
                                WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                                return;
                            }
                        }
                    }
                } else if (WeldTypeConstant.Circle == weldingJobBO.getWeldType()) {
                    // 焊接路径
                    WeldPointBO next = null;
                    for (int i = 0; i < jointPosList.size(); i++) {
                        WeldPointBO weldPointBO = jointPosList.get(i);
                        if (i + 1 < jointPosList.size()) {
                            next = jointPosList.get(i + 1);
                        }
                        if (PositionTypeEnum.CENTER.getCode() == weldPointBO.getPositionType()) {
                            if (RobotHolder.isStopWeld()) {
                                WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                                return;
                            }
                            robot.MoveC(weldPointBO.getJointPos(), weldPointBO.getDescPose(),RobotHolder.getRobot().getRobotPO().getToolCoordinateNum(), RobotHolder.getRobot().getRobotPO().getWorkpieceCoordinateNum(), vel, 100, new ExaxisPos(), 0, new DescPose(),
                                    next.getJointPos(), next.getDescPose(), RobotHolder.getRobot().getRobotPO().getToolCoordinateNum(), RobotHolder.getRobot().getRobotPO().getWorkpieceCoordinateNum(), vel, 100, new ExaxisPos(), 0, new DescPose(), 100, 20);
                            if (RobotHolder.isStopWeld()) {
                                WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                                return;
                            }
                        }
                    }
                }

                // 运动到收弧点
                if (WeldTypeConstant.Line == weldingJobBO.getWeldType()) {
                    if (RobotHolder.isStopWeld()) {
                        WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                        return;
                    }
                    robot.MoveL(arcEndPoint.getJointPos(), arcEndPoint.getDescPose(),RobotHolder.getRobot().getRobotPO().getToolCoordinateNum(), RobotHolder.getRobot().getRobotPO().getWorkpieceCoordinateNum(), vel, 100, 100, -1, new ExaxisPos(), 0, 0, new DescPose(), 0, 100);
                }

                // 停止焊接
                if (weldREQ.getWeldEnabled()) {
                    if (RobotHolder.isStopWeld()) {
                        WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                        return;
                    }
                    robot.ARCEnd(0, RobotHolder.getRobot().getRobotPO().getArcConfigNum(), 10000);
                    ARC = false;
                }

                // 停止摆动
                if (weldREQ.getWeldEnabled()) {
                    if (RobotHolder.isStopWeld()) {
                        WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                        return;
                    }
                    robot.WeaveEnd(RobotHolder.getRobot().getRobotPO().getWeaveConfigNum());
                    weave = false;
                }

                // 运动到结束安全点
                if (null != endSecurePoint) {
//                    robot.MoveJ(endSecurePoint.getJointPos(), endSecurePoint.getDescPose(),1, 0, 20, 100, 100, new ExaxisPos(), 0, 0, new DescPose());
                    if (RobotHolder.isStopWeld()) {
                        WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                        return;
                    }
                    robot.MoveL(endSecurePoint.getJointPos(), endSecurePoint.getDescPose(),RobotHolder.getRobot().getRobotPO().getToolCoordinateNum(), RobotHolder.getRobot().getRobotPO().getWorkpieceCoordinateNum(), 100, 100, 100, -1, new ExaxisPos(), 0, 0, new DescPose(), 0, 100);
                }
            }

            // 停止焊接
            robot.ARCEnd(0, RobotHolder.getRobot().getRobotPO().getArcConfigNum(), 10000);
            ARC = false;

            // 停止摆动
            robot.WeaveEnd(RobotHolder.getRobot().getRobotPO().getWeaveConfigNum());
            weave =false;

        } finally {
            JavafxHolder.setStartWeldDisable(WeldButtonIdEnum.COMMON_GROUP_WELD.getCode(), false);
            RobotHolder.setNoBusy();
            WeldTool.stopWeld(robot, weldREQ, weave, ARC);
            robot.Mode(1);
        }
    }
}
